package com.service.abolishing.dto;

import com.service.abolishing.dao.EnvFactorDao;
import com.service.abolishing.entity.ApDetail;
import com.service.abolishing.entity.EnvFactor;
import com.service.abolishing.entity.LocationResult;
import com.service.abolishing.service.ApDetailService;
import jama.Matrix;
import org.springframework.stereotype.Service;

import javax.annotation.Resource;
import java.sql.Timestamp;
import java.util.*;
import java.util.stream.Collectors;

/**
 * created on 2020年05月08日
 *
 * 三边加权定位算法
 *
 * author  guochuang
 * @version  0.0.1
 */
@Service
public class WeightTrilateral implements Dealer {
	/*环境因子*/
	private EnvFactor envfactor;
	@Resource
    private ApDetailService apDetailService;
	@Resource
	private EnvFactorDao envFactorDao;
	@Override
	public LocationResult getLocation(String str) throws Exception {
		/*实例化定位结果*/
		/*定位结果*/
		LocationResult location = new LocationResult();
		Map map = new HashMap();
		/*分组*/
		DoGroup doGrouper = new DoGroup();
		ArrayList<BleBase> uniqueBases = doGrouper.doGroup(str);

		/*如果收到的基站个数小于3，不能定位，直接返回*/
		if(uniqueBases==null){
			return null;
		}
		//获取rssi值最大的基站id
		String maxRssiBaseId= uniqueBases.get(0).getId();
		ApDetail apDetail = apDetailService.queryByApMac(maxRssiBaseId);
		if (apDetail == null) {
			return null;
		}
		envfactor = envFactorDao.queryByApId(apDetail.getId());
		if (envfactor == null) {
			return null;
		}
		/*拿到终端id*/
		String[] str1 = str.split(";");

		/*求组合数*/
		Integer[] a = doGrouper.getA();
		CombineAlgorithm ca = null;

		try {
			ca = new CombineAlgorithm(a,3);
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		Object[][] c = Objects.requireNonNull(ca).getResult();

		double[] tempLocation = new double[2];

		for(int i = 0; i < c.length; i++){

			/*创建一个列表，用来对每个组合进行计算*/
			List<BleBase> triBases = new ArrayList<BleBase>();

			for(int j = 0; j<3; j++){
				BleBase bb = uniqueBases.get((int) c[i][j]);
				triBases.add(bb);
			}

			/*三个基站为一组通过距离加权后求出的坐标*/
			map = calculate(triBases);
			double[] weightLocation = (double[]) map.get("loc");

			tempLocation[0] += weightLocation[0];
			tempLocation[1] += weightLocation[1];
		}

		location.setTerminalX(tempLocation[0]/(double)map.get("totalWeight"));
		location.setTerminalY(tempLocation[1]/(double)map.get("totalWeight"));

		/*设置定位结果的时间戳*/
		Timestamp ts = new Timestamp(System.currentTimeMillis());
		location.setTimestamp(ts);
		return location;
	}


	/**
	 * 求出通过该组基站距离加权后的坐标
	 *
	 * @param  bases 接收到的一组基站对象列表(此处列表中的基站应当是id各异的)
	 * @return  返回通过该组基站距离加权后的坐标
	 */
	public Map calculate(List<BleBase> bases){
		Map map = new HashMap();
		/*基站的id与坐标*/
		final Map<String, double[]> basesLocation = new HashMap<String, double[]>();

		/*距离数组*/
		double[] distanceArray = new double[bases.size()];

		String[] ids = new String[bases.size()];

		double[] rawLocation;

		double[] loc;
		/*所有组合的总权值*/
		double totalWeight = 0;
		int j = 0;

		/*得到环境影响因素的值*/
		double height = envfactor.getApHeight();
		double n =  envfactor.getApN();
		double p0 =  envfactor.getApP0();
		/*获得基站id*/
		for (BleBase base : bases) {
			ids[j] = base.getId();
			distanceArray[j] = base.getDistance(height, n, p0);
			j++;
		}
		List<ApDetail> apDetailArrayList = new ArrayList<>();
		/*基站的坐标信息应当根据id去redis数据库中查找*/
        apDetailArrayList = apDetailService.queryByIds(ids);
		List<ApDetail> collect = apDetailArrayList.stream().filter(apDetail -> apDetail.getApY() != null && apDetail.getApX() != null).collect(Collectors.toList());
		collect.forEach(apDetail -> {
            double[] loc1 = new double[2];
            loc1[0] = apDetail.getApX();
            loc1[1] = apDetail.getApY();
            basesLocation.put(apDetail.getApMac(),loc1);
        });
		int disArrayLength = distanceArray.length;

		double[][] a = new double[bases.size()-1][2];

		double[][] b = new double[bases.size()-1][1];

		/*数组a初始化*/
		for(int i = 0; i < 2; i ++ ) {
 			a[i][0] = 2*(basesLocation.get(ids[i])[0]-basesLocation.get(ids[bases.size()-1])[0]);
			a[i][1] = 2*(basesLocation.get(ids[i])[1]-basesLocation.get(ids[bases.size()-1])[1]);
		}

		/*数组b初始化*/
		for(int i = 0; i < 2; i ++ ) {
			b[i][0] = Math.pow(basesLocation.get(ids[i])[0], 2)
					- Math.pow(basesLocation.get(ids[bases.size()-1])[0], 2)
					+ Math.pow(basesLocation.get(ids[i])[1], 2)
					- Math.pow(basesLocation.get(ids[bases.size()-1])[1], 2)
					+ Math.pow(distanceArray[disArrayLength-1], 2)
					- Math.pow(distanceArray[i],2);
		}

		/*将数组封装成矩阵*/
		Matrix b1 = new Matrix(b);
		Matrix a1 = new Matrix(a);

		/*求矩阵的转置*/
		Matrix a2  = a1.transpose();

		/*求矩阵a1与矩阵a1转置矩阵a2的乘积*/
		Matrix tmpMatrix1 = a2.times(a1);
		Matrix reTmpMatrix1 = tmpMatrix1.inverse();
		Matrix tmpMatrix2 = reTmpMatrix1.times(a2);

		/*中间结果乘以最后的b1矩阵*/
		Matrix resultMatrix = tmpMatrix2.times(b1);
		double[][] resultArray = resultMatrix.getArray();

		rawLocation = new double[2];

		/*给未加权的结果数组赋值*/
		for(int i = 0; i < 2; i++) {
			rawLocation[i] = resultArray[i][0];
		}

		/*对应的权值*/
		double weight = 0;

		for(int i = 0; i<3; i++){
			weight += (1.0/distanceArray[i]);
		}
		totalWeight += weight;

		/*实例化坐标数组*/
		loc = new double[2];

		/*计算加权过后的坐标*/
		for(int i = 0; i < 2; i++) {
			loc[i] = rawLocation[i]*weight;
		}
		map.put("loc",loc);
		map.put("totalWeight",totalWeight);
		return map;
	}
	/**
	 * 求三角形质心算法
	 *
	 * @param  r1 Round对象
	 * @param  r2
	 * @param  r3
	 * @return  Coordinate对象
	 */
	public static Coordinate triCentroid(Round r1, Round r2, Round r3) {

		/*有效交叉点1*/
		Coordinate p1 = null;
		/*有效交叉点2*/
		Coordinate p2 = null;
		/*有效交叉点3*/
		Coordinate p3 = null;

		/*三点质心坐标*/
		Coordinate centroid = new Coordinate();

		/*r1,r2交点*/
		List<Coordinate> intersections1 = intersection(r1.getX(), r1.getY(), r1.getR(),
				r2.getX(), r2.getY(), r2.getR());

		if (intersections1 != null && !intersections1.isEmpty()) {
			for (Coordinate intersection :intersections1) {
				double pow = Math.pow(intersection.getX() - r3.getX(), 2);
				double pow1 = Math.pow(intersection.getY() - r3.getY(), 2);
				double pow2 = Math.pow(r3.getR(), 2);
				if (p1==null && pow + pow1 <= pow2) {
					p1 = intersection;
				}else if(p1!=null){
					double pow3 = Math.pow(p1.getX()
							- r3.getX(), 2);
					double pow4 = Math.pow(p1.getY() - r3.getY(), 2);
					if(pow1+pow <= pow2){
						if(Math.sqrt(pow+ pow1)>Math.sqrt(pow3 + pow4)){
							p1 = intersection;
						}
					}
				}
			}
		} else {//没有交点定位错误
			return null;
		}

		/*r1,r3交点*/
		List<Coordinate> intersections2 = intersection(r1.getX(), r1.getY(), r1.getR(),
				r3.getX(), r3.getY(), r3.getR());

		if (intersections2 != null && !intersections2.isEmpty()) {
			for (Coordinate intersection : intersections2) {//有交点
				if (p2==null&&Math.pow(intersection.getX()-r2.getX(),2)
						+ Math.pow(intersection.getY()-r2.getY(),2) <= Math.pow(r2.getR(),2)) {
					p2 = intersection;

				}else if(p2!=null){
					if(Math.pow(intersection.getX()-r2.getX(),2) + Math.pow(intersection.getY()
							-r2.getY(),2) <= Math.pow(r2.getR(),2)){
						if(Math.pow(intersection.getX()-r2.getX(),2) + Math.pow(intersection.getY()
								-r2.getY(),2)>Math.sqrt(Math.pow(p2.getX()-r2.getX(),2)
								+ Math.pow(p2.getY()-r2.getY(),2))){
							p1 = intersection;
						}
					}
				}
			}
		} else {//没有交点定位错误
			return null;
		}

		/*r2,r3交点*/
		List<Coordinate> intersections3 = intersection(r2.getX(), r2.getY(), r2.getR(),
				r3.getX(), r3.getY(), r3.getR());

		if (intersections3 != null && !intersections3.isEmpty()) {
			for (Coordinate intersection : intersections3) {//有交点
				if (Math.pow(intersection.getX()-r1.getX(),2)
						+ Math.pow(intersection.getY()-r1.getY(),2) <= Math.pow(r1.getR(),2)) {
					p3 = intersection;
				}else if(p3!=null){
					if(Math.pow(intersection.getX()-r1.getX(),2) + Math.pow(intersection.getY()
							-r1.getY(),2) <= Math.pow(r1.getR(),2)){
						if(Math.pow(intersection.getX()-r1.getX(),2) + Math.pow(intersection.getY()
								-r1.getY(),2)>Math.sqrt(Math.pow(p3.getX()-r1.getX(),2)
								+ Math.pow(p3.getY()-r1.getY(),2))){
							p3 = intersection;
						}
					}
				}
			}
		} else {//没有交点定位错误
			return null;
		}

		/*质心*/
		assert p1 != null;
		assert p2 != null;
		assert p3 != null;
		centroid.setX((p1.getX()+p2.getX()+p3.getX())/3);
		centroid.setY((p1.getY()+p2.getY()+p3.getY())/3);

		return centroid;
	}

	/**
	 * 求两个圆的交点
	 *
	 * @param  x1  圆心1横坐标
	 * @param  y1  圆心1纵坐标
	 * @param  r1  圆心1半径
	 * @param  x2  圆心2横坐标
	 * @param  y2  圆心2纵坐标
	 * @param  r2 圆心2半径
	 * @return 返回两个圆的交点坐标对象列表
	 */
	public static List<Coordinate> intersection(double x1, double y1, double r1,
												double x2, double y2, double r2) {

		double d = Math.sqrt(Math.pow(x1 - x2, 2) + Math.pow(y1 - y2, 2));// 两圆心距离
		List<Coordinate> points  =new ArrayList<Coordinate>();//交点坐标列表
		Coordinate coor;
		double abs = Math.abs(r1 - r2);
		if (d > r1 + r2 || d < abs) {//相离或内含
			return null;
		} else if (x1 == x2 && y1 == y2) { //同心圆
			return null;
		} else if (y1 == y2 && x1 != x2) {
			double a = ((r1 * r1 - r2 * r2) - (x1 * x1 - x2 * x2)) / (2 * x2 - 2 * x1);
			if (d == abs || d == r1 + r2) {// 只有一个交点时
				coor=new Coordinate();
				coor.setX(a);
				coor.setY(y1);
				points.add(coor);
			} else{// 两个交点
				double t = r1 * r1 - (a - x1) * (a - x1);
				coor=new Coordinate();
				coor.setX(a);
				coor.setY(y1 + Math.sqrt(t));
				points.add(coor);
				coor = new Coordinate();
				coor.setX(a);
				coor.setY(y1 - Math.sqrt(t));
				points.add(coor);
			}
		} else if (y1 != y2) {
			double k, disp;
			k = (2 * x1 - 2 * x2) / (2 * y2 - 2 * y1);
			disp = ((r1 * r1 - r2 * r2) - (x1 * x1 - x2 * x2) - (y1 * y1 - y2 * y2))
					/ (2 * y2 - 2 * y1);// 直线偏移量
			double a, b, c;
			a = (k * k + 1);
			b = (2 * (disp - y1) * k - 2 * x1);
			c = (disp - y1) * (disp - y1) - r1 * r1 + x1 * x1;
			double disc;
			disc = b * b - 4 * a * c;// 一元二次方程判别式
			if (d == Math.abs(r1 - r2) || d == r1 + r2) {
				coor=new Coordinate();
				coor.setX((-b) / (2 * a));
				coor.setY(k * coor.getX() + disp);
				points.add(coor);
			} else {
				coor=new Coordinate();
				coor.setX(((-b) + Math.sqrt(disc)) / (2 * a));
				coor.setY(k * coor.getX() + disp);
				points.add(coor);
				coor=new Coordinate();
				coor.setX(((-b) - Math.sqrt(disc)) / (2 * a));
				coor.setY(k * coor.getX() + disp);
				points.add(coor);
			}
		}
		return points;
	}
}
